import pandas as pd
import numpy as np
from scipy.interpolate import LinearNDInterpolator
import warnings

warnings.filterwarnings("ignore", message=".*invalid value encountered in true_divide.*")


############################################# 解析 (x,y) 为 (theta,phi) ###########################################
# 解析 (x,y) 为列
def parse_point(point_str):
    """将字符串 '(x,y)' 解析为 (x, y) 元组"""
    # 去掉括号并分割
    coords = point_str.strip('()').split(',')
    return int(coords[0]), int(coords[1])


# (x_r, y_r) 和 (theta_r, phi_r) 解析 (x_p, y_p) 为 (theta_p, phi_p)
def xy_to_theta_phi(x_pred, y_pred, x_real, y_real, theta_real, phi_real):
    """
    根据标定数据 (x_real, y_real) -> (theta_real, phi_real)
    插值得到任意 (x_pred, y_pred) 对应的 (theta_pred, phi_pred)

    注意：phi（方位角）具有周期性 [0, 360)，需特殊处理
    """
    # 准备标定点
    points = np.column_stack((x_real, y_real))

    # 处理方位角 phi 的周期性：使用正弦和余弦进行插值
    phi_rad = np.radians(phi_real)
    phi_sin = np.sin(phi_rad)
    phi_cos = np.cos(phi_rad)

    # 构建两个插值器：一个用于 theta，一个用于 phi（通过 sin/cos）
    interp_theta = LinearNDInterpolator(points, theta_real, fill_value=np.nan)
    interp_sin = LinearNDInterpolator(points, phi_sin, fill_value=np.nan)
    interp_cos = LinearNDInterpolator(points, phi_cos, fill_value=np.nan)

    # 对预测坐标进行插值
    pred_points = np.column_stack((x_pred, y_pred))

    theta_pred = interp_theta(pred_points)
    sin_pred = interp_sin(pred_points)
    cos_pred = interp_cos(pred_points)

    # 从 sin 和 cos 重构方位角（避免 0°/360° 跳变）
    phi_pred_rad = np.arctan2(sin_pred, cos_pred)
    phi_pred = np.degrees(phi_pred_rad)
    phi_pred = np.mod(phi_pred, 360.0)  # 确保在 [0, 360)

    return theta_pred, phi_pred


############################################# 计算球面夹角误差 ###########################################
# 计算球面夹角误差
def spherical_heading_error(theta_r, phi_r, theta_p, phi_p):
    """
    计算指向角的球面夹角误差（单位：度）

    参数:
    theta_r, phi_r: 真实值的俯仰角和方位角（单位：度）
    theta_p, phi_p: 预测值的俯仰角和方位角（单位：度）

    返回:
    误差（单位：度）
    """
    # 转换为弧度
    theta_r_rad = np.radians(theta_r)
    phi_r_rad = np.radians(phi_r)
    theta_p_rad = np.radians(theta_p)
    phi_p_rad = np.radians(phi_p)

    # 计算单位向量
    x_r = np.cos(theta_r_rad) * np.cos(phi_r_rad)
    y_r = np.cos(theta_r_rad) * np.sin(phi_r_rad)
    z_r = np.sin(theta_r_rad)

    x_p = np.cos(theta_p_rad) * np.cos(phi_p_rad)
    y_p = np.cos(theta_p_rad) * np.sin(phi_p_rad)
    z_p = np.sin(theta_p_rad)

    # 计算点积并确保在[-1, 1]范围内
    dot_product = np.clip(x_r * x_p + y_r * y_p + z_r * z_p, -1.0, 1.0)

    # 计算夹角（弧度）并转换为角度
    error_rad = np.arccos(dot_product)
    error_deg = np.degrees(error_rad)

    return error_deg


############################################# 主函数 ###########################################
def main(path_csv_input, path_csv_output):
    # 读取 CSV 文件
    df = pd.read_csv(path_csv_input)  # 请替换为您的实际文件名

    # 解析 point_ori 和 point_out 列
    df[['x_ori', 'y_ori']] = df['point_ori'].apply(lambda x: pd.Series(parse_point(x)))
    df[['x_out', 'y_out']] = df['point_out'].apply(lambda x: pd.Series(parse_point(x)))

    # 提取标定数据（真实坐标 → 真实角度）
    x_real = df['x_ori'].values
    y_real = df['y_ori'].values
    theta_real = df['trace_th_smooth'].values
    phi_real = df['trace_ph_smooth'].values

    # 提取预测坐标
    x_pred = df['x_out'].values
    y_pred = df['y_out'].values

    # 执行插值反解
    theta_pred, phi_pred = xy_to_theta_phi(
        x_pred, y_pred,
        x_real, y_real,
        theta_real, phi_real
    )

    # 将结果写回 DataFrame
    df['theta_pred'] = theta_pred
    df['phi_pred'] = phi_pred

    # 计算球面夹角误差
    df['error'] = spherical_heading_error(
        df['trace_th_smooth'],
        df['trace_ph_smooth'],
        df['theta_pred'],
        df['phi_pred']
    )

    # 计算统计量
    valid_errors = df['error'].dropna()
    min_error = valid_errors.min()
    max_error = valid_errors.max()
    mean_error = valid_errors.mean()
    std_error = valid_errors.std()

    # 打印统计结果
    print("=" * 50)
    print(f"误差统计分析结果:{path_csv_input}")
    print(f"最小误差: {min_error:.4f} 度")
    print(f"最大误差: {max_error:.4f} 度")
    print(f"平均误差: {mean_error:.4f} 度")
    print(f"标准差: {std_error:.4f} 度")
    print("=" * 50)

    # 保存结果
    df.to_csv(path_csv_output, index=False)




if __name__ == "__main__":
    main('y_out_psll_point-2025-11-11-0.csv', 'y_out_psll_point_thph-2025-11-11-0.csv')
    main('y_out_psll_point-2025-11-11-1.csv', 'y_out_psll_point_thph-2025-11-11-1.csv')
    main('y_out_psll_point-2025-11-11-2.csv', 'y_out_psll_point_thph-2025-11-11-2.csv')
    main('y_out_psll_point-2025-11-11-3.csv', 'y_out_psll_point_thph-2025-11-11-3.csv')
    main('y_out_psll_point-2025-11-11-4.csv', 'y_out_psll_point_thph-2025-11-11-4.csv')